package moro;

/**
 * Class Sensor
 * specialization from class Device
 * contains all Sensors from the robot (Laser + Sonar)
 * @author bleker, van zutphen, stellingwerf, grobben
 * @date oktober 2011
 */
import java.awt.Polygon;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.ArrayList;

public abstract class Sensors extends Device {
	
        public Sensors(String name, Robot robot, Position local, Environment environment) {
            super(name, robot, local, environment);
        }
    
	int orientation = 1;        // 1: clockwise  -1: otherwise
	double rotStep  = 1.0;      // one degree
	double numSteps = 0;
	boolean detect  = false;
	boolean scan    = false;
	//int range = 100;      // maximum range Laser and Sensor
        
        Measure detectMeasure = null;
        ArrayList<Measure> scanMeasures = new ArrayList<Measure>();

        /**
         * Meet de afstand tot een mogelijk object met een Sensor
         * 
         * @param first
         * @return
         */
        public double read(boolean first) {
                int range = getRange();
		Point2D centre = new Point2D.Double(localPos.getX(), localPos.getY());
		Point2D front = new Point2D.Double(localPos.getX() + range * Math.cos(localPos.getT()),
				localPos.getY() + range * Math.sin(localPos.getT()));
                
		// reads the robot's position
		robot.readPosition(robotPos);
		// center's coordinates according to the robot position
		robotPos.rototras(centre);
		// front's coordinates according to the robot position
		robotPos.rototras(front);

		double minDistance = -1.0;
		for(int i=0; i < environment.obstacles.size(); i++) {
                    // This is really dirty: the laser uses direct access to environment's obstacles
                    Obstacle obstacle = (Obstacle) environment.obstacles.get(i);
                    
                    // een laser detecteert geen Opaque obstacles, Sonar wel
                    // er is een overridden methode isOpaqueObstacle in de subklassen
                    if(isOpaqueObstacle(obstacle)) {
                        continue;
                    }
                    // meet de afstand tot het Obstacle
                    // return -1.0 als er niets wordt gedetecteerd 
                    double dist = pointToObstacle(obstacle.polygon, centre, front, first);
                    if(minDistance == -1.0 || (dist > 0 && dist < minDistance)) {
                        minDistance = dist;
			if(minDistance > -1 && first) {
                            return minDistance;
			}
                    }
		}
                
		if(minDistance > 0)  {
                    return minDistance;
                }
                else {
                    return -1.0;
                }
	}

        /**
	 * receives the vertex coordinates of segment beam;
	 * if segment beam intersects an edge of this PhysicalShape, it returns
	 * the distance of the first vertex of beam from the closest edge
	 * if beam does not intersect the PhysicalShape, the return value is -1.0
         * 
         * @param polygon
         * @param centre
         * @param front
         * @param first
         * @return distance to obstacle in centimeters
         */
        public double pointToObstacle(Polygon polygon, Point2D centre, Point2D front, boolean first) {
		int j = 0;
		double minDistance = -1.0;
		double dist = -1.0;
		double px, py;
		double x1, y1, x2, y2;
		double m1, q1, m2, q2;
		Line2D.Double beam = new Line2D.Double(centre, front);

		for(int i=0; i < polygon.npoints; i++) {
                    j = i+1;
                    if(j == polygon.npoints) {
                        j = 0;
                    }
                    x1 = polygon.xpoints[i];
                    y1 = polygon.ypoints[i];
                    x2 = polygon.xpoints[j];
                    y2 = polygon.ypoints[j];
                    if(beam.intersectsLine(x1, y1, x2, y2)) {
                        // calculates the intersection point
                    	if(centre.getX() == front.getX()) {
                            px = centre.getX();
                            py = (y2 - y1) / (x2 - x1) * (px - x1) + y1;
			}
			else
                            if(x1 == x2) {
                                px = x1;
				py = (front.getY()-centre.getY()) / (front.getX()-centre.getX()) * (px - centre.getX()) + centre.getY();
                            }
                            else {
                                m1 = (y2 - y1) / (x2 - x1);
				q1 = y1 - m1 * x1;
				m2 = (front.getY()-centre.getY()) / (front.getX()-centre.getX());
				q2 = centre.getY() - m2 * centre.getX();
				px = (q2 - q1) / (m1 - m2);
				py = m1 * px + q1;
                            }
                            
                            // calculates the distance between (cx, cy) and the intersection point
                            dist = Point2D.Double.distance(centre.getX(), centre.getY(), px, py);
                            if(minDistance == -1.0 || minDistance > dist) {
                                minDistance = dist;
                            }
                            if(first && minDistance > 0.0) {
                                return minDistance;
                            }
                    }
		}
		return minDistance;
	}
        
        /**
         * hier worden de door de Controller ontvangen commando's verwerkt
         * wordt aangeroepen door Device
         * @param command  commando ontvangen van controller
         */
        @Override
	public void executeCommand(String command) {
                // ROTATETO commando
		if(command.indexOf("ROTATETO") > -1) {
                    rotStep = 4.0;
                    double direction = Math.abs(Double.parseDouble(command.trim().substring(9).trim()));
                    while(direction < 0.0) {
                        direction+=360.0;
                    }
                    while(direction > 360.0) {
                        direction-=360.0;
                    }
                    
                    double dirDiff = direction - Math.toDegrees(localPos.getT());   // ??????????????
                    if(dirDiff >= 0.0 && dirDiff <= 180.0) {
                        numSteps = dirDiff / rotStep;
			orientation = 1;
                    }
                    else if(dirDiff >= 0.0 && dirDiff > 180.0) {
                        numSteps = (360.0 - dirDiff) / rotStep;
			orientation = -1;
                    }
                    else if(dirDiff < 0.0 && -dirDiff <= 180.0) {
                        numSteps = -dirDiff / rotStep;
			orientation = -1;
                    }
                    else if(dirDiff < 0.0 && -dirDiff > 180.0) {
                        numSteps = (360.0 + dirDiff) / rotStep;
			orientation = 1;
                    }
                    
                    running = true;
                }
                
                // READ commando. Meet de afstand recht vooruit  (1x dus)       
		else if(command.equalsIgnoreCase("READ")) {
                    writeOut("t=" + Double.toString(this.localPos.getT()) + " d=" + Double.toString(this.read(true)));
                    // ??????????????
		}
                
                // SCAN commando. Meet 360 graden rondom (360x dus)
		else if(command.equalsIgnoreCase("SCAN")) {
                    rotStep = 1.0;
                    scanMeasures.clear();
                    numSteps = 360.0 / rotStep;
                    orientation = 1;
                    scan = true;
                    // send the list of measures
                    commands.add("GETMEASURES");
                    running = true;
		}
		else if(command.equalsIgnoreCase("GETMEASURES")) {
                    Measure measure = null;
                    String measures = "SCAN";
                    for(int i=0; i < scanMeasures.size(); i++) {
                        measure = scanMeasures.get(i);
			measures += " d=" + measure.distance + " t=" + measure.direction;
                    }
                    writeOut(measures);
		}
                        
                // DETECT commando: scant van -45 tot +45 graden recht naar voren.  (90x dus)
                // stopt zodra een obstacle is gedetecteerd
		else if(command.equalsIgnoreCase("DETECT")) {
                    detect = true;
                    rotStep = 8.0;
                    if(detectMeasure != null) {
                        writeOut("LASER DETECT d=" + detectMeasure.distance + " t=" + detectMeasure.direction);
			detectMeasure = null;
                    }
                    else if(localPos.getT() == Math.toRadians(45.0)) {   // ?????????????
                        // move the laser to the left position
			commands.add("ROTATETO 315");
			// repeats this command
			commands.add("DETECT");
                    }
                    else if(localPos.getT() == Math.toRadians(315.0)) {  // ??????????????
                        // move the laser to the right position
			commands.add("ROTATETO 45");
			// repeats this command
			commands.add("DETECT");
                    }
                    else {
                        // move the laser to the right position
			commands.add("ROTATETO 45");
			// repeats this command
			commands.add("DETECT");
                    }
		}
                        
                //
		else {
                    writeOut("DECLINED");
                }
	}

        /**
         * NextStep 
         * wordt aangeroepen door Device
         * hier worden de door de Controller ontvangen commando's verwerkt
         * voor elk device. Steeds één stap per aanroep
         */
        @Override
	public void nextStep() {
		if(running && numSteps > 0.0) {
                    if(numSteps < 1.0) {
                        localPos.rototras(0.0, 0.0, orientation*numSteps*rotStep);
                    }
                    else {
                        localPos.rototras(0.0, 0.0, orientation*rotStep);
                    }
                    // teken Environment opnieuw
                    environment.repaint();
                    // -------------------------
                    
                    numSteps-=1.0;
                    running = true;
		}
		else if(running) {
                    running = false;
                    if(!detect && !scan) {
                        writeOut("LASER ARRIVED");
                    }
		}
		if(detect) {
                    double distance = this.read(true);
                    if(distance > -1.0) {
                        if(detectMeasure == null) {
                            detectMeasure = new Measure(distance, localPos.getT());
                        }  // ?????????????
                        else if(detectMeasure.distance > distance) {
                            detectMeasure.set(distance, localPos.getT());
                        }  // ????????????
                    }
		}
		else if(scan) {
                    double distance = this.read(false);
                    if(distance > -1.0) {
                        scanMeasures.add(new Measure(distance, localPos.getT()));
                    }  // ??????????????
		}
	}
        
        // overridden method in Laser and Sensor
        public abstract boolean isOpaqueObstacle(Obstacle obstacle);

        @Override
        public abstract int getRange(); 
        
	// Innerclass Measure represents measurements from the Laser and Sonar
	class Measure {
		public double distance = 0;
		public double direction = 0.0;
		
                Measure(double dist, double dir) {
                    distance = dist;
                    direction = dir;
                    while(direction >= 2.0*Math.PI) {
                        direction -= 2.0*Math.PI;
                    }
                    
                    while(direction < 0.0) {
                        direction += 2.0*Math.PI;
                    }
		}
		
                public void set(double dist, double dir) {
                    distance = dist;
                    direction = dir;
                    while(direction >= 2.0*Math.PI) {
                        direction -= 2.0*Math.PI;
                    }
                    
                    while(direction < 0.0) {
                        direction += 2.0*Math.PI;
                    }
		}
	}
}
